#include <Arduino.h>
#include <Esp32McpwmMotor.h>
#include <Esp32PcntEncoder.h>
#include <PidController.h>
#include <Kinematics.h>

Esp32McpwmMotor motor;           // 创建一个名为motor的对象，用于控制电机
Esp32PcntEncoder encoders[2];    // 创建一个数组用于存储两个编码器
PidController pid_controller[2]; // 创建PID控制器对象数组
Kinematics kinematics;

float target_linear_speed = 50.0;  // 目标线速度（单位：mm/s）
float target_angular_speed = 0.1f; // 目标角速度（单位：rad/s）
float out_left_speed;
float out_right_speed;

// put function declarations here:

void setup()
{
    // put your setup code here, to run once:
    Serial.begin(115200);
    encoders[0].init(0, 32, 33);  // 初始化第一个编码器，使用 GPIO 32 和 33 连接
    encoders[1].init(1, 26, 25);  // 初始化第一个编码器，使用 GPIO 26 和 25 连接
    motor.attachMotor(0, 22, 23); // 将电动机0连接到引脚22和引脚23
    motor.attachMotor(1, 12, 13); // 将电动机1连接到引脚12和引脚13
    // 初始化PID控制器参数
    pid_controller[0].update_pid(0.625, 0.125, 0.0);
    pid_controller[1].update_pid(0.625, 0.125, 0.0);
    pid_controller[0].out_limit(-100, 100);
    pid_controller[1].out_limit(-100, 100);
    // 初始化轮子间距和电机参数
    kinematics.set_wheel_distance(175);
    kinematics.set_motor_param(0, 0.1051566);
    kinematics.set_motor_param(1, 0.1051566);
    // 运动学逆解并设置速度
    kinematics.kinematic_inverse(target_linear_speed, target_angular_speed, out_left_speed, out_right_speed);
    // 初始化目标速度，单位mm/s,使用毫米防止浮点运算丢失精度
    pid_controller[0].upate_target(out_left_speed);
    pid_controller[1].upate_target(out_right_speed);
}

void loop()
{
    // put your main code here, to run repeatedly:
    delay(10);
    kinematics.update_motor_speed(millis(), encoders[0].getTicks(), encoders[1].getTicks());
    motor.updateMotorSpeed(0, pid_controller[0].update(kinematics.get_motor_speed(0)));
    motor.updateMotorSpeed(1, pid_controller[1].update(kinematics.get_motor_speed(1)));
}

// put function definitions here:
